#include <utils.hpp>
//
#include <remoter_cali.hpp>

RemoterCali::RemoterCali(int state)
	: m_State(state)
	, m_Tk(0)
	, m_Step(0)
	, m_CaliWaitTk(100)
	, m_CaliWaitStep(5)
	, m_CaliFilter(0.1)
{
	memset(&m_RcRawBegin, 0, sizeof(msg_rc_raw_s));
	memset(&m_RcRawEnd, 0, sizeof(msg_rc_raw_s));
	memset(&m_RcRawCurr, 0, sizeof(double) * MSG_RC_E_CNT);
	memset(&m_RcRawPrev, 0, sizeof(double) * MSG_RC_E_CNT);
}

RemoterCali::~RemoterCali()
{
}

void RemoterCali::Reset(void)
{
	m_Tk   = 0;
	m_Step = 0;
	memset(&m_RcRawBegin, 0, sizeof(msg_rc_raw_s));
	memset(&m_RcRawEnd, 0, sizeof(msg_rc_raw_s));
	memset(&m_RcRawCurr, 0, sizeof(double) * MSG_RC_E_CNT);
	memset(&m_RcRawPrev, 0, sizeof(double) * MSG_RC_E_CNT);
}

void RemoterCali::Process(msg_rc_raw_s* rcRaw, msg_rc_s* rc)
{
	m_Tk++;
}

double RemoterCali::CalcValueByParams(uint16_t val, uint16_t min, uint16_t mid, uint16_t max, uint8_t reverse)
{
	if (reverse == 1)
	{
		std::swap(min, max);
		mid = max - mid + min;
		val = max - val + min;
	}

	if (!(min <= mid && mid <= max))
	{
		return 0.0f;
	}

	val = LimitInt(val, min, max);
	if (val == mid)
	{
		return 0.0f;
	}

	if (val < mid)
	{
		if (mid == min)
		{
			return 0.0f;
		}
		return (double)(val - mid) / (double)(mid - min);
	}

	if (val > mid)
	{
		if (mid == max)
		{
			return 1.0f;
		}
		return (double)(val - mid) / (double)(max - mid);
	}

	return 0.0f;
}

void RemoterCali::CalcValueChangedChannel(uint8_t& changeChannel, uint16_t& changeValue, msg_rc_raw_s* rcRaw0, msg_rc_raw_s* rcRaw1)
{
	int8_t ch	 = 0;
	uint16_t val = 0;

	for (int i = 0; i < 4; i++)
	{
		uint16_t v = abs(rcRaw0->rc_raw[i] - rcRaw1->rc_raw[i]);
		if (val < v)
		{
			val = v;
			ch	= i;
		}
	}

	changeChannel = ch;
	changeValue	  = rcRaw1->rc_raw[ch];
}